/* 
 * File:   main.cpp
 * Author: wen
 *
 * Created on June 22, 2010, 5:21 PM
 */

/*
 * main.cpp
 *
 *  Created on: Jun 22, 2010
 *      Author: wen
 */
#include <iostream>
#include <math.h>
#include <itpp/itcomm.h>
#include <itpp/itbase.h>

using namespace itpp;
using namespace std;

//These lines are needed for use of cout and endl
using std::cout;
using std::endl;

int main() 
{

  // Open the recorded data file
   vec magx, magy, magz;
   mat magxyz;

   it_file mag;
   mag.open("path_outside_50x10_zigzag_try1.it");
   mag >> Name("m") >> magxyz;
   mag.close();

   magx = magxyz.get_row(0);
   magy = magxyz.get_row(1);
   magz = magxyz.get_row(2);

   // Plot the magnetometer data to see whether they are on ellipsoid.
   //cout << "magnetometer's x axis is" << magx << endl;
   //cout << "its length is " << magx.length() << endl;
   it_file mag_matlab;
   mag_matlab.open("plot_mag.it");
   mag_matlab << Name("mx") << magx;
   mag_matlab << Name("my") << magy;
   mag_matlab << Name("mz") << magz;
   mag_matlab.flush();
   mag_matlab.close();

   /*
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    *               Magnetometer Calibration
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    */

   // %%% Generate D matrix
   vec mx2 = elem_mult(magx,magx);
   vec my2 = elem_mult(magy,magy);
   vec mz2 = elem_mult(magz,magz);
   vec myz = 2*elem_mult(magy,magz);
   vec mxz = 2*elem_mult(magx,magz);
   vec mxy = 2*elem_mult(magx,magy);
   mat D = zeros(10,mx2.length());
   D.set_row(0,mx2);
   D.set_row(1,my2);
   D.set_row(2,mz2);
   D.set_row(3,myz);
   D.set_row(4,mxz);
   D.set_row(5,mxy);
   D.set_row(6,2*magx);
   D.set_row(7,2*magy);
   D.set_row(8,2*magz);
   D.set_row(9,ones(mx2.length()));
   mat DT=D*D.transpose();

   // %% Generate C matrix
   int k = 4;
   int s = k/2 - 1;
   mat C11 = -eye(3);
   mat C12 = eye(3);
   C11.set(0,1,s);
   C11.set(0,2,s);
   C11.set(1,0,s);
   C11.set(1,2,s);
   C11.set(2,0,s);
   C11.set(2,1,s);
   C12.set(0,0,-k);
   C12.set(1,1,-k);
   C12.set(2,2,-k);
   mat C1 = zeros(6,6);
   C1.set_submatrix(0,2,0,2,C11);
   C1.set_submatrix(3,5,3,5,C12);
   mat C = zeros(10,10);
   C.set_submatrix(0,5,0,5,C1);

   // %% Generate the S matrice
   mat S1 = DT.get_cols(0,5);
   mat S2 = DT.get_cols(6,9);
   mat S11 = S1.get_rows(0,5);
   mat S12 = S2.get_rows(0,5);
   mat S22 = S2.get_rows(6,9);

   // %% Calculate the eigenvectors and eigenvalues
   mat A = inv(C1) * (S11 - S12 * inv(S22) * S12.transpose());
   cvec r;
   cmat V;
   eig(A, r, V);
   int maxindex = max_index(real(r));
   vec v1 = real(V).get_col(maxindex);
   vec v2 = - inv(S22)*S12.transpose()*v1;
   vec v = concat(v1,v2);

   // %% Generate Q matrix and u vector.
   mat Q = zeros(3,3);
   vec u = zeros(3);
   Q.set(0,0,v(0));
   Q.set(1,1,v(1));
   Q.set(2,2,v(2));
   Q.set(1,2,v(3));
   Q.set(2,1,v(3));
   Q.set(0,2,v(4));
   Q.set(2,0,v(4));
   Q.set(0,1,v(5));
   Q.set(1,0,v(5));

   u.set(0,v(6));
   u.set(1,v(7));
   u.set(2,v(8));

   float kk = v(9);

   // %% Calculate the combined bias vector b
   vec b = 0.5 * inv(Q) * u;

   // %% Calculate Hm2
   //float Hm2 = b * Q * b -k;
   mat bb = zeros(1,3);
   bb.set_row(0,b);
   float Hm2 = (bb * Q * bb.transpose()).get(0,0) + kk;
   //cout << bb * Q * bb.transpose() + kk << endl;

   // %% Calculate eigenvalues D_Q matrix and eigenvecctors V_Q matrix and alpha
   mat QT = Q * Q.transpose();
   cvec d_Q;
   cmat V_Q_c;
   eig(QT, d_Q, V_Q_c);
   mat D_Q = zeros(3,3);
   D_Q.set(0,0,real(d_Q(0)));
   D_Q.set(1,1,real(d_Q(1)));
   D_Q.set(2,2,real(d_Q(2)));
   mat V_Q = real(V_Q_c);
   mat uu = zeros(1,3);
   uu.set_row(0, u);
   mat x1 = (V_Q.transpose()*uu.transpose()).transpose();
   mat x2 = x1 * inv(D_Q) * x1.transpose();
   float alpha = Hm2/(4*kk - x2(0,0));


   // %% Print the parameters for calibration
   cout << "V = " << V_Q << endl;
   cout << "alpha= " << alpha << endl;
   cout << "D = " << D_Q << endl;
   cout << "b= " << b << endl;

   // %% calculate the calibrated magnetometer data
   mat hhat = zeros(3,magx.length());
   hhat.set_row(0,magx);
   hhat.set_row(1,magy);
   hhat.set_row(2,magz);

   mat bbb = zeros(3,magx.length());
   for(int i=0; i<=magx.length(); i++)
   {
       bbb.set_row(i,b);
   }
   mat h;
   h = V_Q * real(sqrtm(alpha*D_Q)) * V_Q.transpose()*(hhat + bbb);
   
   // %% plot calibrated magnetometer data
   it_file magh;
   magh.open("plot_calibrated_mag.it");
   magh << Name("mx") << h.get_row(0);
   magh << Name("my") << h.get_row(1);
   magh << Name("mz") << h.get_row(2);
   magh.flush();
   magh.close();

   // %% Exit program:
   cout << "Done" << endl;
  return 0;

}